package robot.devices;

import java.sql.Time;
import java.util.Date;
import java.util.Timer;

import robot.Robot;
import robot.devices.Wheel.WHEEL_DIR;
import robot.devices.Wheel.WHEEL_MODE;
import robot.exception.RobotException;
import robot.protocol.MODULE;
import robot.utils.NotChecked;

public final class Wheels {
	private Wheel leftWheel;
	private Wheel rightWheel;
	
	private int actualSpeed = 0;
	private WHEEL_DIR direction;
	private double angle;
	
	public Wheels(Robot robot) throws RobotException {
		leftWheel = new Wheel(robot, MODULE.MOTOR_LEFT, WHEEL_MODE.MOT_ASSERV_PWM, WHEEL_DIR.WHEEL_FORWARD);
		rightWheel = new Wheel(robot, MODULE.MOTOR_RIGHT, WHEEL_MODE.MOT_ASSERV_PWM, WHEEL_DIR.WHEEL_BACKWARD);
		this.setSpeed(0);
	}

	public int getSpeed() {
		return actualSpeed;
	}

	public void setSpeed(int value) throws RobotException {
		if (value < 0){
			leftWheel.setDirection(WHEEL_DIR.WHEEL_BACKWARD);
			rightWheel.setDirection(WHEEL_DIR.WHEEL_FORWARD);
			value = -value;
		} else {
			leftWheel.setDirection(WHEEL_DIR.WHEEL_FORWARD);
			rightWheel.setDirection(WHEEL_DIR.WHEEL_BACKWARD);
		}
		this.actualSpeed = value;
		leftWheel.setSpeed(actualSpeed);
		rightWheel.setSpeed(actualSpeed);
	}


	public void setSpeedInPercent(int percent) throws RobotException {
		this.actualSpeed = (percent * leftWheel.maxSpeed) / 100;
		leftWheel.setSpeedPercent(percent);
		rightWheel.setSpeedPercent(percent);
	}
	
	public WHEEL_DIR getDirection() {
		return direction;
	}

	public void setDirection(WHEEL_DIR direction) throws RobotException {
		this.direction = direction;
		leftWheel.setDirection(direction);
		if (direction == WHEEL_DIR.WHEEL_BACKWARD)
			rightWheel.setDirection(WHEEL_DIR.WHEEL_FORWARD);
		else
			rightWheel.setDirection(WHEEL_DIR.WHEEL_BACKWARD);
	}

	public double getAngle() {
		return angle;
	}

	public void setAngle(double angle) throws RobotException {
		this.angle = angle;
		
		leftWheel.setAsservMode(WHEEL_MODE.MOT_ASSERV_POS);
		leftWheel.setDestination(0x100);
	}

	@NotChecked
	public void rotate(int angle) throws RobotException {
		int value = Math.abs((angle * 550) / 70);
		
		long time = 0;
		int speed;
		
		this.halt();
		if (angle > 0) {
			speed = 70;
		} else {
			speed = -70;
		}
		
		
		leftWheel.setSpeed(speed);
		time = System.currentTimeMillis();
		while (System.currentTimeMillis() < time + value)
			;	// rotate
	
		leftWheel.setSpeed(0);
		
	}
	
	public void halt() throws RobotException {
		leftWheel.halt();
		rightWheel.halt();
	}

	public Wheel getLeftWheel() {
		return leftWheel;
	}

	public void setLeftWheel(Wheel leftWheel) {
		this.leftWheel = leftWheel;
	}

	public Wheel getRightWheel() {
		return rightWheel;
	}

	public void setRightWheel(Wheel rightWheel) {
		this.rightWheel = rightWheel;
	}

	public void setAsservMode(WHEEL_MODE asserv) throws RobotException {
		this.leftWheel.setAsservMode(asserv);
		this.rightWheel.setAsservMode(asserv);
	}

	public void refresh() throws RobotException {
		this.leftWheel.refresh();
		this.rightWheel.refresh();
	}
	
	public final static int CENTIMETERS = 410;
	
	public long getOdometrie () throws RobotException {
		return (((leftWheel.getOdometrie() + rightWheel.getOdometrie()) / 2) / CENTIMETERS);
	}
}
